#include "03imu2euler.hpp"

void callbackImu(const sensor_msgs::ImuConstPtr &msg)
{
    imu_lock.lock();
    imuQue.push(msg);
    imu_lock.unlock();

    imu_bf_lock.lock();
    imu_bf.emplace_back(*msg);
    imu_bf_lock.unlock();
}

void imuEulerIntegral()
{   
    bool first_imu = true;
    Eigen::Quaterniond last_q, cur_q;
    Eigen::Vector3d last_acc, last_gyr;
    double last_t, cur_t;
    while (1)
    {
        if(imuQue.size() < 2)
        {
            // std::cout << "0000000" << std::endl;
            continue;
        }
            
        
        imu_lock.lock();  
        sensor_msgs::ImuConstPtr imu_msg= imuQue.front();
        imuQue.pop();
        imu_lock.unlock();
        
        double cur_t = imu_msg->header.stamp.toSec();
        Eigen::Vector3d cur_acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
        Eigen::Vector3d cur_gyr(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
        Eigen::Quaterniond cur_quat(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);
        
        // 初始姿态
        static Eigen::Quaterniond quat_init(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);

        // std::cout << std::fixed << std::setprecision(2) << quat_init.coeffs().transpose() << std::endl;
        // std::cout << std::fixed << std::setprecision(2) << "" << (cur_quat.toRotationMatrix().eulerAngles(2, 1, 0) * 180 / M_PI).transpose() << std::endl;
        
        if(first_imu)
        {
            last_q = quat_init;
            cur_q  = quat_init;
            last_t = imu_msg->header.stamp.toSec();

            last_acc = cur_acc;
            last_gyr = cur_gyr;
        }

        Eigen::Quaterniond delta_q(1, 0.5*cur_gyr.x(), 0.5*cur_gyr(1), 0.5*cur_gyr[2]);
        // 微分方程求解 q1 = delta(q)*q0;  角速度表示 q1 = [1, 0.5 * delta(w1)*dt] * q0; 四元素乘法
        // q1 = q0 + q'*q0; q' = [0, 0.5 * delta(w)t]   欧拉法： fx = f0 + f' * x
        cur_q = delta_q * last_q;   // 左乘
        cur_q.normalize();          // 单位化：旋转约束

        first_imu = false;
        last_t = cur_t;
        last_q = cur_q;

        Eigen::Vector3d euler = cur_q.toRotationMatrix().eulerAngles(2,1,0);
        
        std::cout << std::fixed << std::setprecision(2) << "EulerIntegral: " 
            << (euler * 180 / M_PI).transpose() << std::endl;
        // euler_gyr = euler;
        std::chrono::milliseconds dura(10);  // 毫秒
        std::this_thread::sleep_for(dura);   // 当前线程暂定执行dura毫秒

    }    
}

void imuMedianIntegral()
{   
    bool first_imu = true;
    Eigen::Vector3d last_euler, cur_euler;
    Eigen::Vector3d last_acc, last_gyr;
    double last_t, cur_t;

    while (1)
    {
        if(imu_bf.size() < 2)
        {
            continue;
        }

        imu_bf_lock.lock();
        sensor_msgs::Imu imu_msg = imu_bf.front();
        imu_bf.pop_front();
        imu_bf_lock.unlock();

        double cur_t = imu_msg.header.stamp.toSec();
        Eigen::Vector3d cur_acc(imu_msg.linear_acceleration.x, imu_msg.linear_acceleration.y, imu_msg.linear_acceleration.z);
        Eigen::Vector3d cur_gyr(imu_msg.angular_velocity.x, imu_msg.angular_velocity.y, imu_msg.angular_velocity.z);
        static Eigen::Quaterniond cur_quat1(imu_msg.orientation.w, imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z);

        if(first_imu)
        {
            last_t = cur_t;
            last_gyr = cur_gyr;
            cur_euler = cur_quat1.toRotationMatrix().eulerAngles(2, 1, 0);
            last_euler = cur_euler;
        }

        double dt = cur_t - last_t;
        if(dt > 0.0001)
        cur_euler = last_euler + 0.5 * (last_gyr + cur_gyr) * dt;

        // 更新
        first_imu  = false;
        last_t     = cur_t;
        last_euler = cur_euler;
        last_gyr   = cur_gyr;

        std::cout << std::fixed << std::setprecision(2) << "MedianIntegral: " 
            << (cur_euler * 180 / M_PI).transpose() << std::endl;
        
        // euler_gyr = cur_euler;

        std::chrono::milliseconds dura(10);  // 毫秒
        std::this_thread::sleep_for(dura);   // 当前线程暂定执行dura毫秒

    }
    
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "thread_node");
    ros::NodeHandle n;
    
    ros::Subscriber sub_img = n.subscribe("/imu/data", 1000, &callbackImu);

    std::thread t1(imuEulerIntegral);
    std::thread t2(imuMedianIntegral);

    // while(1)
    // {   
        
    //     Eigen::Vector3d e = euler1 + euler2;
    // }

    // std::cout << "---------------" << std::endl;

    ros::spin();
    return 0;
}